///\todo upgrade

//////////////////////////////////////////////////////////////////////////////////
//                                                                              //
//  This file is part of the buola project (https://code.google.com/p/buola/).  //
//                                                                              //
//  Copyright(c) 2007-2012 Xavi Gratal                                          //
//  gratal AT gmail DOT com                                                     //
//                                                                              //
//  Buola is free software: you can redistribute it and/or modify               //
//  it under the terms of the GNU General Public License as published by        //
//  the Free Software Foundation, either version 3 of the License, or           //
//  (at your option) any later version.                                         //
//                                                                              //
//  Buola is distributed in the hope that it will be useful,                    //
//  but WITHOUT ANY WARRANTY; without even the implied warranty of              //
//  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               //
//  GNU General Public License for more details.                                //
//                                                                              //
//  You should have received a copy of the GNU General Public License           //
//  along with buola.  If not, see <http://www.gnu.org/licenses/>.              //
//                                                                              //
//////////////////////////////////////////////////////////////////////////////////

#ifndef _BUOLA_MAT_CRRT_H_
#define _BUOLA_MAT_CRRT_H_

#include <buola/mat.h>
#include <buola/utility/random.h>

namespace buola { namespace mat {

template<typename tScalar>
class CRRT
{
public:
    typedef mat::CVec<tScalar> TPoint;

    CRRT()
    {}

    CRRT(const TPoint &pInit)
    {
        mPoints.push_back(pInit);
        mParents.push_back(0);
    }

    void AddPoint(const TPoint &pPoint,int pParent)
    {
        mPoints.push_back(pPoint);
        mParents.push_back(pParent);
    }

    int NearestNeighbour(const TPoint &pPoint)
    {
        int lMin=0;
        auto lMinDist=distance(mPoints[lMin],pPoint);

        for(int i=1;i<mPoints.size();i++)
        {
            auto lDist=distance(mPoints[i],pPoint);
            if(lDist<lMinDist)
            {
                lMinDist=lDist;
                lMin=i;
            }
        }

        return lMin;
    }

    int size() const {   return mPoints.size();  }
    const TPoint &Point(int i) const {   return mPoints[i];   }
    const TPoint &Parent(int i) const {   return mPoints[mParents[i]];   }

private:
    std::vector<TPoint> mPoints;
    std::vector<int> mParents;
};

template<typename tScalar,typename tSpace>
CRRT<tScalar> rrt(const mat::CVec<tScalar> &pInit,int pK,tScalar pDelta,const tSpace &pSpace)
{
    CRRT<tScalar> lRRT(pInit);
    
    for(int i=0;i<pK;i++)
    {
        auto lXRand=pSpace.RandomPoint();

        int lNN=lRRT.NearestNeighbour(lXRand);
        mat::CVec<tScalar> lDiff=lXRand-lRRT.Point(lNN);
        lDiff/=norm(lDiff);

        mat::CVec<tScalar> lNewPoint=lRRT.Point(lNN)+pDelta*lDiff;
        if(pSpace.Occupied(lNewPoint))
        {
            i--;
            continue;
        }
        
        lRRT.AddPoint(lRRT.Point(lNN)+pDelta*lDiff,lNN);
    }

    return lRRT;
}

/*namespace mat*/ } /*namespace buola*/ }

#endif
